﻿/*                       NOTICE
 * 
 * This software was produced for the U. S. Government
 * under Contract No. FA8721-10-C-0001, and is
 * subject to the Rights in Noncommercial Computer Software
 * and Noncommercial Computer Software Documentation Clause
 * (DFARS) 252.227-7014 (JUN 1995)
 * 
 * Approved for Public Release: 11-2435
 * © 2011 The MITRE Corporation. All Rights Reserved. 
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 * http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Kinect;
using TUIO;
using CCT.NUI.HandTracking;

namespace GRIP
{
    /// <summary>
    /// KinectAdapter is the GRIP adapter for retrieving full-body skeleton and finger contacts from a user. It uses
    /// Microsoft's Kinect SDK to get the contacts, optionally furthers that data by finding fingers, and converts the
    /// information into a set of publicly accessible TuioContact objects.
    /// </summary>
    public class KinectAdapter : ITuioAdapter
    {
        #region Class Fields
        /// <summary>
        /// Holds the various modes for finger tracking.
        /// </summary>
        public enum FingerModes
        {
            /// <summary>
            /// Finger tracking will be disabled; only the SDK's skeleton points will be used.
            /// </summary>
            Off,

            /// <summary>
            /// Finger tracking will be synchronized with the skeleton, so the adapter will wait until
            /// tracking is complete for each frame before sending a new contact frame.
            /// </summary>
            Synchronous,

            /// <summary>
            /// Finger tracking will be asynchronous and decoupled from the rest of the skeleton data.
            /// The finger points and skeleton points update independently, whenever they are ready.
            /// </summary>
            Asynchronous
        };

        /// <summary>
        /// Holds the various modes for the Kinect's color camera.
        /// </summary>
        public enum RGBModes
        {
            /// <summary>
            /// Color camera will be disabled.
            /// </summary>
            Off,

            /// <summary>
            /// Color camera will be enabled at 640x480, 30 FPS.
            /// </summary>
            On640x480,

            /// <summary>
            /// Color camera will be enabled at 1280x960, 12 FPS.
            /// </summary>
            On1280x960
        }

        /// <summary>
        /// The current mode of finger tracking
        /// </summary>
        [Setting]
        public FingerModes FingerMode;

        /// <summary>
        /// True to allow "guesses" from the Kinect SDK, where it isn't confident in a joint location
        /// False to only allow confirmed contacts and disable guesses
        /// </summary>
        [Setting]
        public bool AllowInferredContacts;

        /// <summary>
        /// The name of the application using this adapter (used for optional TUIO messages)
        /// </summary>
        [Setting]
        public string Source;

        private KinectSensor Nui;
        private FingerFinder LeftFingerFinder;
        private FingerFinder RightFingerFinder;
        private short[] DepthFrame = new short[640 * 480];
        private byte[] ColorFrame;
        private BufferFilter<TuioContact> filter;
        protected long ContactID = 0;
        private int[] FakeColorIndices;
        public bool UseRealtimeImageAlignment = true;
        public bool UsePlayerCutout = true;
        protected Dictionary<TuioContactType, TuioContact> contacts = new Dictionary<TuioContactType, TuioContact>();
        private EventHandler<AllFramesReadyEventArgs> KinectEventHandler;

        /// <summary>
        /// This event is fired whenever the contact data has been updated and a new frame is ready.
        /// </summary>
        public event EventHandler<ContactFrameReadyEventArgs> ContactFrameReady;

        #endregion

        #region Public Contacts
        /// <summary>The left hand thumb contact</summary>
        public TuioContact ThumbLeft;

        /// <summary>The left hand index finger contact</summary>
        public TuioContact IndexFingerLeft;

        /// <summary>The left hand middle finger contact</summary>
        public TuioContact MiddleFingerLeft;

        /// <summary>The left hand ring finger contact</summary>
        public TuioContact RingFingerLeft;

        /// <summary>The left hand pinky contact</summary>
        public TuioContact PinkyLeft;

        /// <summary>The right hand thumb contact</summary>
        public TuioContact ThumbRight;

        /// <summary>The right hand index finger contact</summary>
        public TuioContact IndexFingerRight;

        /// <summary>The right hand middle finger contact</summary>
        public TuioContact MiddleFingerRight;

        /// <summary>The right hand ring finger contact</summary>
        public TuioContact RingFingerRight;

        /// <summary>The right hand pinky contact</summary>
        public TuioContact PinkyRight;

        /// <summary>The central hip contact</summary>
        public TuioContact HipCenter;

        /// <summary>The spine (body) contact</summary>
        public TuioContact Spine;

        /// <summary>The central shoulder (chest) contact</summary>
        public TuioContact ShoulderCenter;

        /// <summary>The head contact</summary>
        public TuioContact Head;

        /// <summary>The left shoulder contact</summary>
        public TuioContact ShoulderLeft;

        /// <summary>The left elbow contact</summary>
        public TuioContact ElbowLeft;

        /// <summary>The left wrist contact</summary>
        public TuioContact WristLeft;

        /// <summary>The left hand contact</summary>
        public TuioContact HandLeft;

        /// <summary>The right hsoulder contact</summary>
        public TuioContact ShoulderRight;

        /// <summary>The right elbow contact</summary>
        public TuioContact ElbowRight;

        /// <summary>The right wrist contact</summary>
        public TuioContact WristRight;

        /// <summary>The right hand contact</summary>
        public TuioContact HandRight;

        /// <summary>The left hip contact</summary>
        public TuioContact HipLeft;

        /// <summary>The left knee contact</summary>
        public TuioContact KneeLeft;

        /// <summary>The left ankle contact</summary>
        public TuioContact AnkleLeft;

        /// <summary>The left foot contact</summary>
        public TuioContact FootLeft;

        /// <summary>The right hip contact</summary>
        public TuioContact HipRight;

        /// <summary>The right knee contact</summary>
        public TuioContact KneeRight;

        /// <summary>The right ankle contact</summary>
        public TuioContact AnkleRight;

        /// <summary>The right foot contact</summary>
        public TuioContact FootRight;
        #endregion

        #region Initialization
        /// <summary>
        /// Creates a new adapter with the given Kinect device settings,
        /// and the given finger tracking settings.
        /// </summary>
        /// <param name="settings"></param>
        /// <param name="fingerSettings"></param>
        protected KinectAdapter()
        {
            InitializeContacts();
        }

        public static KinectAdapter CreateAdapter(KinectSettings settings, FingerSettings fingerSettings)
        {
            KinectAdapter kinect = new KinectAdapter();
            kinect.Nui = (from sensorToCheck in KinectSensor.KinectSensors 
                          where sensorToCheck.Status == KinectStatus.Connected 
                            select sensorToCheck).FirstOrDefault();
            kinect.FingerMode = settings.FingerMode;
            kinect.AllowInferredContacts = settings.AllowInferredContacts;
            kinect.Source = kinect.Nui.DeviceConnectionId;
            kinect.filter = new BufferFilter<TuioContact>(settings.KinectEntryBufferTime, settings.KinectExitBufferTime);
            kinect.UseRealtimeImageAlignment = settings.UseRealtimeImageAlignment;

            if (settings.RGBMode == RGBModes.Off)
            {
                kinect.Nui.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
                kinect.Nui.SkeletonStream.ChooseSkeletons(1);
                kinect.Nui.SkeletonStream.Enable(new TransformSmoothParameters()
                {
                    Smoothing = 1.0f,
                    Correction = 0.1f,
                    Prediction = 0.8f,
                    JitterRadius = 0.03f,
                    MaxDeviationRadius = 0.01f
                });
            }
            else if (settings.RGBMode == RGBModes.On640x480)
            {
                kinect.Nui.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
                kinect.Nui.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
                kinect.Nui.SkeletonStream.Enable();/*new TransformSmoothParameters()
                {
                    Smoothing = 1.0f,
                    Correction = 0.1f,
                    Prediction = 0.8f,
                    JitterRadius = 0.03f,
                    MaxDeviationRadius = 0.01f
                });*/
                kinect.ColorFrame = new byte[640 * 480 * 4];
            }
            else if (settings.RGBMode == RGBModes.On1280x960)
            {
                kinect.Nui.ColorStream.Enable(ColorImageFormat.RgbResolution1280x960Fps12);
                kinect.Nui.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
                kinect.Nui.SkeletonStream.ChooseSkeletons(1);
                kinect.Nui.SkeletonStream.Enable(new TransformSmoothParameters()
                {
                    Smoothing = 1.0f,
                    Correction = 0.1f,
                    Prediction = 0.8f,
                    JitterRadius = 0.03f,
                    MaxDeviationRadius = 0.01f
                });
            }

            kinect.LeftFingerFinder = new FingerFinder(kinect, FingerFinder.HandType.Left, fingerSettings);
            kinect.RightFingerFinder = new FingerFinder(kinect, FingerFinder.HandType.Right, fingerSettings);

            kinect.KinectEventHandler = new EventHandler<AllFramesReadyEventArgs>(kinect.Nui_AllFramesReady);
            kinect.Nui.AllFramesReady += kinect.KinectEventHandler;

            return kinect;
        }

        private void CalculateFixedCutout()
        {
            FakeColorIndices = new int[307200];
            short[] fakeDepth = new short[307200];
            for (int i = 0; i < 307200; i++)
                fakeDepth[i] = 9600;
            ColorImagePoint[] points = new ColorImagePoint[307200];
            Nui.MapDepthFrameToColorFrame(DepthImageFormat.Resolution640x480Fps30, fakeDepth, ColorImageFormat.RgbResolution640x480Fps30, points);
            for (int i = 0; i < points.Length; i++)
            {
                int index = (points[i].Y * 640 + points[i].X) * 4;
                if(index < 0 || index > 1228799)
                    index = 0;
                FakeColorIndices[i] = index;
            }
        }

        protected void InitializeContacts()
        {
            contacts.Add(TuioContactType.ThumbLeft, ThumbLeft = new TuioContact(Source, ContactID++, 0, 0, 0, 0, TuioContactType.ThumbLeft));
            contacts.Add(TuioContactType.IndexFingerLeft, IndexFingerLeft = new TuioContact(Source, ContactID++, 1, 0, 0, 0, TuioContactType.IndexFingerLeft));
            contacts.Add(TuioContactType.MiddleFingerLeft, MiddleFingerLeft = new TuioContact(Source, ContactID++, 2, 0, 0, 0, TuioContactType.MiddleFingerLeft));
            contacts.Add(TuioContactType.RingFingerLeft, RingFingerLeft = new TuioContact(Source, ContactID++, 3, 0, 0, 0, TuioContactType.RingFingerLeft));
            contacts.Add(TuioContactType.PinkyLeft, PinkyLeft = new TuioContact(Source, ContactID++, 4, 0, 0, 0, TuioContactType.PinkyLeft));
            contacts.Add(TuioContactType.ThumbRight, ThumbRight = new TuioContact(Source, ContactID++, 5, 0, 0, 0, TuioContactType.ThumbRight));
            contacts.Add(TuioContactType.IndexFingerRight, IndexFingerRight = new TuioContact(Source, ContactID++, 6, 0, 0, 0, TuioContactType.IndexFingerRight));
            contacts.Add(TuioContactType.MiddleFingerRight, MiddleFingerRight = new TuioContact(Source, ContactID++, 7, 0, 0, 0, TuioContactType.MiddleFingerRight));
            contacts.Add(TuioContactType.RingFingerRight, RingFingerRight = new TuioContact(Source, ContactID++, 8, 0, 0, 0, TuioContactType.RingFingerRight));
            contacts.Add(TuioContactType.PinkyRight, PinkyRight = new TuioContact(Source, ContactID++, 9, 0, 0, 0, TuioContactType.PinkyRight));
            contacts.Add(TuioContactType.HipCenter, HipCenter = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HipCenter));
            contacts.Add(TuioContactType.Spine, Spine = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.Spine));
            contacts.Add(TuioContactType.ShoulderCenter, ShoulderCenter = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ShoulderCenter));
            contacts.Add(TuioContactType.Head, Head = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.Head));
            contacts.Add(TuioContactType.ShoulderLeft, ShoulderLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ShoulderLeft));
            contacts.Add(TuioContactType.ElbowLeft, ElbowLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ElbowLeft));
            contacts.Add(TuioContactType.WristLeft, WristLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.WristLeft));
            contacts.Add(TuioContactType.HandLeft, HandLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HandLeft));
            contacts.Add(TuioContactType.ShoulderRight, ShoulderRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ShoulderRight));
            contacts.Add(TuioContactType.ElbowRight, ElbowRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.ElbowRight));
            contacts.Add(TuioContactType.WristRight, WristRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.WristRight));
            contacts.Add(TuioContactType.HandRight, HandRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HandRight));
            contacts.Add(TuioContactType.HipLeft, HipLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HipLeft));
            contacts.Add(TuioContactType.KneeLeft, KneeLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.KneeLeft));
            contacts.Add(TuioContactType.AnkleLeft, AnkleLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.AnkleLeft));
            contacts.Add(TuioContactType.FootLeft, FootLeft = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.FootLeft));
            contacts.Add(TuioContactType.HipRight, HipRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.HipRight));
            contacts.Add(TuioContactType.KneeRight, KneeRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.KneeRight));
            contacts.Add(TuioContactType.AnkleRight, AnkleRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.AnkleRight));
            contacts.Add(TuioContactType.FootRight, FootRight = new TuioContact(Source, ContactID++, -1, 0, 0, 0, TuioContactType.FootRight));
        }
        #endregion

        #region Skeleton Processing
        private void Nui_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            using (DepthImageFrame newDepthFrame = e.OpenDepthImageFrame())
            {
                using (ColorImageFrame newColorFrame = e.OpenColorImageFrame())
                {
                    using (SkeletonFrame newSkeletonFrame = e.OpenSkeletonFrame())
                    {
                        if (newDepthFrame == null || newColorFrame == null || newSkeletonFrame == null)
                            return;

                        short[] rawDepthValues = new short[newDepthFrame.PixelDataLength];
                        byte[] rawColorValues = new byte[newColorFrame.PixelDataLength];
                        Skeleton[] skeletonData = new Skeleton[newSkeletonFrame.SkeletonArrayLength];
                        
                        Task depthCopyTask = Task.Factory.StartNew(() => newDepthFrame.CopyPixelDataTo(rawDepthValues));
                        Task colorCopyTask = Task.Factory.StartNew(() => newColorFrame.CopyPixelDataTo(rawColorValues));
                        Task skeletonCopyTask = Task.Factory.StartNew(() => newSkeletonFrame.CopySkeletonDataTo(skeletonData));

                        depthCopyTask.Wait();
                        colorCopyTask.Wait();
                        if(UseRealtimeImageAlignment)
                            Task.Factory.StartNew(() => ProcessColorFrame(rawDepthValues, rawColorValues));
                        else
                            Task.Factory.StartNew(() => ProcessColorFrame_Fake(rawColorValues));

                        skeletonCopyTask.Wait();
                        if (UsePlayerCutout)
                            ProcessDepthFrame_CutoutEnabled(rawDepthValues, skeletonData);
                        else
                            ProcessDepthFrame(rawDepthValues, skeletonData);
                    }
                }
            }
        }

        private void ProcessColorFrame_Fake(byte[] rawColorValues)
        {
            for (int i = 0; i < 307200; i++)
            {
                int newColorIndex = FakeColorIndices[i];
                int colorFrameIndex = i * 4;
                ColorFrame[colorFrameIndex] = rawColorValues[newColorIndex];
                ColorFrame[colorFrameIndex + 1] = rawColorValues[newColorIndex + 1];
                ColorFrame[colorFrameIndex + 2] = rawColorValues[newColorIndex + 2];
                ColorFrame[colorFrameIndex + 3] = 255;
            }
        }

        private void ProcessDepthFrame_CutoutEnabled(short[] rawDepthValues, Skeleton[] skeletonData)
        {
            Skeleton trackedSkeleton = null;
            int skeletonIndex = -1;
            float minDepth = 100000;

            for (int i = 0; i < skeletonData.Length; i++)
            {
                Skeleton s = skeletonData[i];
                if (s.TrackingState == SkeletonTrackingState.Tracked)
                {
                    float valueZ = s.Position.Z;
                    if (valueZ < minDepth)
                    {
                        minDepth = valueZ;
                        trackedSkeleton = s;
                        skeletonIndex = i + 1;
                    }
                }
            }

            for (int i = 0; i < 307200; i++)
            {
                short rawDepth = rawDepthValues[i];
                if ((rawDepth & DepthImageFrame.PlayerIndexBitmask) == skeletonIndex)
                    DepthFrame[i] = (short)(rawDepth >> DepthImageFrame.PlayerIndexBitmaskWidth);
                else
                    DepthFrame[i] = 0;
            }

            if (trackedSkeleton != null)
                ProcessSkeletonFrame(trackedSkeleton);
                //Task.Factory.StartNew(() => ProcessSkeletonFrame(trackedSkeleton));
        }
        private void ProcessDepthFrame(short[] rawDepthValues, Skeleton[] skeletonData)
        {
            Skeleton trackedSkeleton = null;
            int skeletonIndex = -1;
            float minDepth = 100000;

            for (int i = 0; i < skeletonData.Length; i++)
            {
                Skeleton s = skeletonData[i];
                if (s.TrackingState == SkeletonTrackingState.Tracked)
                {
                    float valueZ = s.Position.Z;
                    if (valueZ < minDepth)
                    {
                        minDepth = valueZ;
                        trackedSkeleton = s;
                        skeletonIndex = i + 1;
                    }
                }
            }

            for (int i = 0; i < 307200; i++)
                DepthFrame[i] = (short)(rawDepthValues[i] >> DepthImageFrame.PlayerIndexBitmaskWidth);

            if (trackedSkeleton != null)
                ProcessSkeletonFrame(trackedSkeleton);
            //Task.Factory.StartNew(() => ProcessSkeletonFrame(trackedSkeleton));
        }

        private void ProcessColorFrame(short[] rawDepthValues, byte[] rawColorValues)
        {
            int length = rawDepthValues.Length;
            ColorImagePoint[] colorMap = new ColorImagePoint[length];
            Nui.MapDepthFrameToColorFrame(DepthImageFormat.Resolution640x480Fps30, rawDepthValues, ColorImageFormat.RgbResolution640x480Fps30, colorMap);
            
            for (int i = 0; i < length; i++)
            {
                ColorImagePoint point = colorMap[i];
                int newColorIndex = (point.Y * 640 + point.X) * 4;
                int colorFrameIndex = i * 4;
                if (newColorIndex > -1 && newColorIndex < 1228800)
                {
                    ColorFrame[colorFrameIndex] = rawColorValues[newColorIndex];
                    ColorFrame[colorFrameIndex + 1] = rawColorValues[newColorIndex + 1];
                    ColorFrame[colorFrameIndex + 2] = rawColorValues[newColorIndex + 2];
                }
                else
                {
                    ColorFrame[colorFrameIndex] = 0;
                    ColorFrame[colorFrameIndex + 1] = 0;
                    ColorFrame[colorFrameIndex + 2] = 0;
                }
            }

        }

        /*
        unsafe private void nui_DepthFrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            Task.Factory.StartNew(() =>
            {
                fixed (byte* p = e.ImageFrame.Image.Bits)
                {
                    fixed (short* s = DepthFrame)
                    {
                        short* newFramePointer = (short*)p;                       
                        short* depthFramePointer = s;

                        for (int y = 0; y < 307200; y += 640)
                        {
                            for (short x = 639; x > -1; x--)
                                *(depthFramePointer++) = *(newFramePointer + x);
                            newFramePointer += 640;
                        }
                    }
                }
            });
        }*/

        private void getJointPosition(Joint joint, TuioContact contact, List<TuioContact> livingContacts)
        {
            if (joint.TrackingState == JointTrackingState.Tracked || (AllowInferredContacts && joint.TrackingState == JointTrackingState.Inferred))
            {
                DepthImagePoint point = Nui.MapSkeletonPointToDepth(joint.Position, DepthImageFormat.Resolution640x480Fps30);
                int depthX = Math.Min(Math.Max(point.X, 0), 639);
                int depthY = Math.Min(Math.Max(point.Y, 0), 479);
                int index = (depthY * 640 + depthX);

                int realDepth = DepthFrame[index];
                if(realDepth == 0)
                    return;

                contact.xpos = (float)depthX / 639.0f;
                contact.ypos = (float)depthY / 479.0f;
                contact.zpos = realDepth;

                livingContacts.Add(contact);
            }
        }

        private void ProcessSkeletonFrame(Skeleton skeleton)
        {
            List<TuioContact> livingContacts = new List<TuioContact>(20);

            JointCollection joints = skeleton.Joints;
            getJointPosition(joints[JointType.HandLeft], HandLeft, livingContacts);
            Task leftHandTask = null;
            Task rightHandTask = null;
            if (FingerMode != FingerModes.Off && HandLeft.active)
            {
                leftHandTask = new Task(LeftFingerFinder.processHand);
                leftHandTask.Start();
            }
            getJointPosition(joints[JointType.HandRight], HandRight, livingContacts);
            if (FingerMode != FingerModes.Off && HandRight.active)
            {
                rightHandTask = new Task(RightFingerFinder.processHand);
                rightHandTask.Start();
            }
            getJointPosition(joints[JointType.HipCenter], HipCenter, livingContacts);
            getJointPosition(joints[JointType.Spine], Spine, livingContacts);
            getJointPosition(joints[JointType.ShoulderCenter], ShoulderCenter, livingContacts);
            getJointPosition(joints[JointType.Head], Head, livingContacts);
            getJointPosition(joints[JointType.ShoulderLeft], ShoulderLeft, livingContacts);
            getJointPosition(joints[JointType.ElbowLeft], ElbowLeft, livingContacts);
            getJointPosition(joints[JointType.WristLeft], WristLeft, livingContacts);
            getJointPosition(joints[JointType.ShoulderRight], ShoulderRight, livingContacts);
            getJointPosition(joints[JointType.ElbowRight], ElbowRight, livingContacts);
            getJointPosition(joints[JointType.WristRight], WristRight, livingContacts);
            getJointPosition(joints[JointType.HipLeft], HipLeft, livingContacts);
            getJointPosition(joints[JointType.KneeLeft], KneeLeft, livingContacts);
            getJointPosition(joints[JointType.AnkleLeft], AnkleLeft, livingContacts);
            getJointPosition(joints[JointType.FootLeft], FootLeft, livingContacts);
            getJointPosition(joints[JointType.HipRight], HipRight, livingContacts);
            getJointPosition(joints[JointType.KneeRight], KneeRight, livingContacts);
            getJointPosition(joints[JointType.AnkleRight], AnkleRight, livingContacts);
            getJointPosition(joints[JointType.FootRight], FootRight, livingContacts);

            if (FingerMode == FingerModes.Synchronous)
            {
                if (leftHandTask != null)
                    leftHandTask.Wait();
                if (rightHandTask != null)
                    rightHandTask.Wait();
            }
                
            if (ThumbLeft.active)
                livingContacts.Add(ThumbLeft);
            if (IndexFingerLeft.active)
                livingContacts.Add(IndexFingerLeft);
            if (MiddleFingerLeft.active)
                livingContacts.Add(MiddleFingerLeft);
            if (RingFingerLeft.active)
                livingContacts.Add(RingFingerLeft);
            if (PinkyLeft.active)
                livingContacts.Add(PinkyLeft);
            if (ThumbRight.active)
                livingContacts.Add(ThumbRight);
            if (IndexFingerRight.active)
                livingContacts.Add(IndexFingerRight);
            if (MiddleFingerRight.active)
                livingContacts.Add(MiddleFingerRight);
            if (RingFingerRight.active)
                livingContacts.Add(RingFingerRight);
            if (PinkyRight.active)
                livingContacts.Add(PinkyRight);

            BufferedFrame<TuioContact> holder = filter.ProcessNewFrame(livingContacts);
            foreach (TuioContact contact in holder.NewObjects)
            {
                contact.active = true;
                contact.session_id = ContactID++;
            }
            foreach (TuioContact contact in holder.RemovedObjects)
                contact.active = false;
            if (ContactFrameReady != null)
            {
                ContactFrameReadyEventArgs args = new ContactFrameReadyEventArgs(
                    holder.NewObjects,
                    holder.ExistingObjects,
                    holder.RemovedObjects,
                    holder.InactiveObjects,
                    DateTime.Now,
                    Source);
                ContactFrameReady(this, args);
            }
        }
        #endregion

        #region Getters
        /// <summary>
        /// Returns the collection of each kinect contact, active or not.
        /// </summary>
        /// <returns>A read-only collection of all contacts</returns>
        public IEnumerable<TuioContact> GetContacts()
        {
            return contacts.Values;
        }

        /// <summary>
        /// Returns the Kinect SDK's Runtime object
        /// </summary>
        /// <returns>The Runtime object representing the Kinect device</returns>
        public KinectSensor GetRuntime()
        {
            return Nui;
        }

        /// <summary>
        /// Returns the last recorded depth frame. It is 640 * 480 * 2, a pixel's true depth can
        /// be accessed by left-shifting the high byte by 8 and logical ORing it with the low byte.
        /// Example: int depth3 = (DepthFrame[6] &lt;&lt; 8) | DepthFrame[5]
        /// </summary>
        /// <returns>The byte array of raw pixel data from the depth camera</returns>
        public short[] GetDepthFrame()
        {
            return DepthFrame;
        }

        public byte[] GetColorFrame()
        {
            return ColorFrame;
        }

        /// <summary>
        /// Returns the point cloud representing the left hand's contour curve.
        /// </summary>
        /// <returns>A point collection describing the left hand outline</returns>
        public Contour GetLeftContour()
        {
            return LeftFingerFinder.GetContour();
        }

        /// <summary>
        /// Returns the point cloud representing the right hand's contour curve.
        /// </summary>
        /// <returns>A point collection describing the right hand outline</returns>
        public Contour GetRightContour()
        {
            return RightFingerFinder.GetContour();
        }

        /// <summary>
        /// Returns the point cloud representing the left hand's convex hull.
        /// </summary>
        /// <returns>A point collection describing the left hand convex hull</returns>
        public ConvexHull GetLeftHull()
        {
            return LeftFingerFinder.GetConvexHull();
        }

        /// <summary>
        /// Returns the point cloud representing the right hand's convex hull.
        /// </summary>
        /// <returns>A point collection describing the right hand convex hull</returns>
        public ConvexHull GetRightHull()
        {
            return RightFingerFinder.GetConvexHull();
        }

        public string GetKinectId()
        {
            return Nui.DeviceConnectionId;
        }

        #endregion

        #region Start and Stop

        public void Start()
        {
            Nui.Start();
            if(FakeColorIndices == null)
                CalculateFixedCutout();
            Nui.AllFramesReady += KinectEventHandler;
        }

        /// <summary>
        /// Turns off the Kinect's IR camera and unregisters the device.
        /// </summary>
        public void Shutdown()
        {
            Nui.AllFramesReady -= KinectEventHandler;
            /*System.Timers.Timer timer = new System.Timers.Timer(500);
            timer.Elapsed += new System.Timers.ElapsedEventHandler((object sender, System.Timers.ElapsedEventArgs e) =>
            {
                Nui.Stop();
                timer.Stop();
            });
            timer.Start();*/
            Nui.Stop();
        }
        #endregion

        #region Explicit Kinect Interaction
        /*
        unsafe public void ProcessDepthFrame()
        {
            ImageFrame img = Nui.DepthStream.GetNextFrame(0);
            if (img == null)
                return;
            fixed (byte* p = img.Image.Bits)
            {
                short[] frame;
                //if (!UseDepthA)
                    frame = DepthFrame;
                //else
                //    frame = DepthFrameB;

                fixed (short* s = frame)
                {
                    short* newFramePointer = (short*)p;
                    short* depthFramePointer = s;

                    for (int y = 0; y < 307200; y += 640)
                    {
                        for (short x = 639; x > -1; x--)
                            *(depthFramePointer++) = *(newFramePointer + x);
                        newFramePointer += 640;
                    }
                }
               // UseDepthA = !UseDepthA;
            }
        }*/

        /*
        public void ProcessSkeletonFrame()
        {
            JointsCollection joints;
            SkeletonFrame skeletonFrame = Nui.SkeletonEngine.GetNextFrame(0);
            if (skeletonFrame == null)
                return;
            List<TuioContact> livingContacts = new List<TuioContact>(20);

            float minZ = float.MaxValue;
            int minID = -1;
            int id = 0;
            foreach (SkeletonData data in skeletonFrame.Skeletons)
            {
                if (SkeletonTrackingState.Tracked == data.TrackingState)
                {
                    Joint joint = data.Joints[JointID.Spine];
                    float depth = joint.Position.Z;
                    if (depth != 0 && depth < minZ)
                    {
                        minZ = depth;
                        minID = id;
                    }
                }
                id++;
            }

            if (minID > -1)
            {
                SkeletonData data = skeletonFrame.Skeletons[minID];
                joints = data.Joints;
                getJointPosition(joints[JointID.HandLeft], HandLeft, livingContacts);
                Task leftHandTask = null;
                Task rightHandTask = null;
                if (FingerMode != FingerModes.Off && HandLeft.active)
                {
                    leftHandTask = new Task(LeftFingerFinder.processHand);
                    leftHandTask.Start();
                }
                getJointPosition(joints[JointID.HandRight], HandRight, livingContacts);
                if (FingerMode != FingerModes.Off && HandRight.active)
                {
                    rightHandTask = new Task(RightFingerFinder.processHand);
                    rightHandTask.Start();
                }
                getJointPosition(joints[JointID.HipCenter], HipCenter, livingContacts);
                getJointPosition(joints[JointID.Spine], Spine, livingContacts);
                getJointPosition(joints[JointID.ShoulderCenter], ShoulderCenter, livingContacts);
                getJointPosition(joints[JointID.Head], Head, livingContacts);
                getJointPosition(joints[JointID.ShoulderLeft], ShoulderLeft, livingContacts);
                getJointPosition(joints[JointID.ElbowLeft], ElbowLeft, livingContacts);
                getJointPosition(joints[JointID.WristLeft], WristLeft, livingContacts);
                getJointPosition(joints[JointID.ShoulderRight], ShoulderRight, livingContacts);
                getJointPosition(joints[JointID.ElbowRight], ElbowRight, livingContacts);
                getJointPosition(joints[JointID.WristRight], WristRight, livingContacts);
                getJointPosition(joints[JointID.HipLeft], HipLeft, livingContacts);
                getJointPosition(joints[JointID.KneeLeft], KneeLeft, livingContacts);
                getJointPosition(joints[JointID.AnkleLeft], AnkleLeft, livingContacts);
                getJointPosition(joints[JointID.FootLeft], FootLeft, livingContacts);
                getJointPosition(joints[JointID.HipRight], HipRight, livingContacts);
                getJointPosition(joints[JointID.KneeRight], KneeRight, livingContacts);
                getJointPosition(joints[JointID.AnkleRight], AnkleRight, livingContacts);
                getJointPosition(joints[JointID.FootRight], FootRight, livingContacts);

                if (FingerMode == FingerModes.Synchronous)
                {
                    if (leftHandTask != null)
                        leftHandTask.Wait();
                    if (rightHandTask != null)
                        rightHandTask.Wait();
                }

                if (ThumbLeft.active)
                    livingContacts.Add(ThumbLeft);
                if (IndexFingerLeft.active)
                    livingContacts.Add(IndexFingerLeft);
                if (MiddleFingerLeft.active)
                    livingContacts.Add(MiddleFingerLeft);
                if (RingFingerLeft.active)
                    livingContacts.Add(RingFingerLeft);
                if (PinkyLeft.active)
                    livingContacts.Add(PinkyLeft);
                if (ThumbRight.active)
                    livingContacts.Add(ThumbRight);
                if (IndexFingerRight.active)
                    livingContacts.Add(IndexFingerRight);
                if (MiddleFingerRight.active)
                    livingContacts.Add(MiddleFingerRight);
                if (RingFingerRight.active)
                    livingContacts.Add(RingFingerRight);
                if (PinkyRight.active)
                    livingContacts.Add(PinkyRight);
            }

            BufferedFrame<TuioContact> holder = filter.ProcessNewFrame(livingContacts);
            foreach (TuioContact contact in holder.NewObjects)
            {
                contact.active = true;
                contact.session_id = ContactID++;
            }
            foreach (TuioContact contact in holder.RemovedObjects)
                contact.active = false;
            if (ContactFrameReady != null)
            {
                ContactFrameReadyEventArgs args = new ContactFrameReadyEventArgs(
                    holder.NewObjects,
                    holder.ExistingObjects,
                    holder.RemovedObjects,
                    holder.InactiveObjects,
                    DateTime.Now,
                    Source);
                ContactFrameReady(this, args);
            }
        }
         * */
        #endregion
    }
}
